import numpy as np
import math
import pandas as pd
from scipy.spatial.distance import cdist
import time

# -------------------------- 1. 基础参数与复用函数 --------------------------
# 定义场景中的关键坐标和参数
target_true = np.array([0, 200, 0])  # 真目标的位置坐标 (x, y, z)
r, h = 7, 10  # 真目标的半径7m、高10m（圆柱形目标）
target_fake = np.array([0, 0, 0])  # 假目标的位置坐标
missile_M1 = np.array([20000, 0, 2000])  # M1导弹的初始位置
missile_speed = 300  # 导弹速度 (m/s)
drone_FY1 = np.array([17800, 0, 1800])  # FY1无人机的初始位置
g = 9.8  # 重力加速度 (m/s²)
smoke_radius = 10  # 烟幕半径 (m)
smoke_sink_speed = 3  # 烟幕下沉速度 (m/s)
smoke_valid_time = 20  # 烟幕有效时间 (s)
NUM_SMOKE = 3  # 烟幕弹数量

# 计算导弹飞到假目标的时间
missile_distance_to_fake = np.linalg.norm(target_fake - missile_M1)
missile_time_to_fake = missile_distance_to_fake / missile_speed  # 导弹到达假目标的时间 (s)


def get_cylinder_bounding_box_vertices(target_center, radius, height):
    """
    计算圆柱形目标的边界长方体顶点
    """
    x_min = target_center[0] - radius
    x_max = target_center[0] + radius
    y_min = target_center[1] - radius
    y_max = target_center[1] + radius
    z_min = target_center[2]
    z_max = target_center[2] + height

    vertices = [
        np.array([x_min, y_min, z_min]),
        np.array([x_max, y_min, z_min]),
        np.array([x_max, y_max, z_min]),
        np.array([x_min, y_max, z_min]),
        np.array([x_min, y_min, z_max]),
        np.array([x_max, y_min, z_max]),
        np.array([x_max, y_max, z_max]),
        np.array([x_min, y_max, z_max])
    ]
    return vertices


# 预计算目标顶点
target_vertices = get_cylinder_bounding_box_vertices(target_true, r, h)


def missile_real_time_position(missile_init_pos, t):
    """
    计算导弹在时间t的位置
    """
    if t >= missile_time_to_fake:
        return target_fake
    if t < 0:
        return missile_init_pos

    dir_vec = target_fake - missile_init_pos
    dir_norm = np.linalg.norm(dir_vec)
    if dir_norm <= 1e-6:
        return missile_init_pos

    dir_unit_vec = dir_vec / dir_norm
    return missile_init_pos + missile_speed * t * dir_unit_vec


def get_smoke_explosion_center(drone_init_pos, drone_speed, theta, t_drop, t_detonate):
    """
    计算烟幕弹爆炸中心点坐标
    """
    drop_x = drone_init_pos[0] + drone_speed * math.cos(theta) * t_drop
    drop_y = drone_init_pos[1] + drone_speed * math.sin(theta) * t_drop
    drop_z = drone_init_pos[2]

    explosion_x = drop_x + drone_speed * math.cos(theta) * t_detonate
    explosion_y = drop_y + drone_speed * math.sin(theta) * t_detonate

    fall_distance = 0.5 * g * (t_detonate ** 2)
    explosion_z = max(drop_z - fall_distance, 0.0)

    return np.array([explosion_x, explosion_y, explosion_z])


def smoke_real_time_center(drone_init_pos, drone_speed, theta, t_drop, t_detonate, t):
    """
    计算烟幕在时间t的中心位置
    """
    detonate_t = t_drop + t_detonate
    if t < detonate_t - 1e-6:
        return np.array([0, 0, 0])

    explosion_center = get_smoke_explosion_center(drone_init_pos, drone_speed, theta, t_drop, t_detonate)
    sink_distance = smoke_sink_speed * (t - detonate_t)
    real_z = max(explosion_center[2] - sink_distance, 0.0)

    return np.array([explosion_center[0], explosion_center[1], real_z])


def is_segment_sphere_intersect(missile_pos, vertex_pos, smoke_center, smoke_radius):
    """
    判断导弹视线是否与烟幕球体相交
    """
    vec_AC = smoke_center - missile_pos
    vec_AB = vertex_pos - missile_pos
    ab_sq_mag = np.dot(vec_AB, vec_AB)

    if ab_sq_mag < 1e-12:
        return False

    k = np.dot(vec_AC, vec_AB) / ab_sq_mag

    if k <= 0.0:
        min_dist = np.linalg.norm(vec_AC)
    elif 0.0 < k < 1.0:
        cross_prod = np.cross(vec_AC, vec_AB)
        min_dist = np.linalg.norm(cross_prod) / np.sqrt(ab_sq_mag)
    else:
        vec_BC = smoke_center - vertex_pos
        min_dist = np.linalg.norm(vec_BC)

    return min_dist <= smoke_radius + 1e-6


def is_effective_occlusion(t, drone_speed, theta, t_drop, t_detonate):
    """
    判断在时间t烟幕是否有效遮蔽目标
    """
    detonate_t = t_drop + t_detonate
    smoke_start = detonate_t
    smoke_end = detonate_t + smoke_valid_time

    if t < smoke_start - 1e-6 or t > smoke_end + 1e-6:
        return False

    missile_pos = missile_real_time_position(missile_M1, t)
    smoke_center = smoke_real_time_center(drone_FY1, drone_speed, theta, t_drop, t_detonate, t)

    for vertex in target_vertices:
        if is_segment_sphere_intersect(missile_pos, vertex, smoke_center, smoke_radius):
            return True

    return False


# -------------------------- 2. DE+PSO混合算法实现 --------------------------
class DE_PSO_Hybrid:
    def __init__(self, pop_size, num_dimensions, bounds, drone_init_pos):
        """
        DE+PSO混合算法初始化
        """
        self.pop_size = pop_size
        self.num_dimensions = num_dimensions
        self.bounds = bounds
        self.drone_init_pos = drone_init_pos

        # 初始化种群
        self.population = self.initialize_population()
        self.velocity = np.zeros((pop_size, num_dimensions))

        # 最优解记录
        self.pbest = self.population.copy()
        self.pbest_fitness = np.full(pop_size, -np.inf)
        self.gbest = None
        self.gbest_fitness = -np.inf
        self.gbest_history = []

        # 算法参数
        self.w = 0.9  # 惯性权重
        self.c1 = 2.0  # 个体学习因子
        self.c2 = 2.0  # 社会学习因子
        self.F = 0.8  # DE缩放因子
        self.CR = 0.9  # DE交叉概率

        # 适应度计算参数
        self.time_step = 0.1  # 时间步长
        self.single_durations = [0.0] * NUM_SMOKE

    def initialize_population(self):
        """
        初始化满足约束的种群
        """
        population = []
        while len(population) < self.pop_size:
            candidate = []
            for i in range(NUM_SMOKE):
                candidate += [
                    np.random.uniform(70, 140),  # 无人机速度 [70, 140]
                    np.random.uniform(0, 2 * math.pi),  # 方向角 [0, 2π]
                    np.random.uniform(0, 30),  # 投放延迟 [0, 30]
                    np.random.uniform(0, 10)  # 起爆延迟 [0, 10]
                ]

            # 检查投放间隔约束
            if self.check_drop_constraints(candidate):
                population.append(candidate)

        return np.array(population)

    def check_drop_constraints(self, candidate):
        """
        检查投放时间间隔约束
        """
        t_drop_list = [candidate[i * 4 + 2] for i in range(NUM_SMOKE)]
        t_drop_list.sort()

        for i in range(len(t_drop_list) - 1):
            if t_drop_list[i + 1] - t_drop_list[i] < 1.0:
                return False
        return True

    def repair_solution(self, solution):
        """
        修复违反约束的解
        """
        t_drop_list = [solution[i * 4 + 2] for i in range(NUM_SMOKE)]
        t_drop_list.sort()

        # 确保投放时间间隔至少1秒
        for i in range(len(t_drop_list) - 1):
            if t_drop_list[i + 1] - t_drop_list[i] < 1.0:
                t_drop_list[i + 1] = t_drop_list[i] + 1.0 + np.random.random() * 0.5

        # 更新解中的投放时间
        for i in range(NUM_SMOKE):
            solution[i * 4 + 2] = t_drop_list[i]

        # 确保参数在边界内
        for i in range(self.num_dimensions):
            low, high = self.bounds[i]
            if solution[i] < low:
                solution[i] = low + np.random.random() * (high - low) * 0.1
            elif solution[i] > high:
                solution[i] = high - np.random.random() * (high - low) * 0.1

        return solution

    def fitness_function(self, solution):
        """
        适应度函数：计算联合遮蔽时长
        """
        # 解析解向量
        params = []
        for i in range(NUM_SMOKE):
            base = i * 4
            drone_speed = solution[base]
            theta = solution[base + 1]
            t_drop = solution[base + 2]
            t_detonate = solution[base + 3]
            params.append((drone_speed, theta, t_drop, t_detonate))

        # 检查投放间隔约束
        if not self.check_drop_constraints(solution):
            return 0.0

        # 统计每枚烟幕弹的单独干扰时长
        single_durations = []
        for i in range(NUM_SMOKE):
            drone_speed, theta, t_drop, t_detonate = params[i]
            detonate_t = t_drop + t_detonate
            smoke_start = detonate_t
            smoke_end = detonate_t + smoke_valid_time

            total_duration = 0.0
            prev_effective = False
            t = smoke_start

            while t <= smoke_end + 1e-6:
                current_effective = is_effective_occlusion(t, drone_speed, theta, t_drop, t_detonate)

                if current_effective and not prev_effective:
                    start_t = t
                elif not current_effective and prev_effective:
                    total_duration += t - start_t

                prev_effective = current_effective
                t += self.time_step

            if prev_effective:
                total_duration += smoke_end - start_t

            single_durations.append(round(total_duration, 3))

        self.single_durations = single_durations

        # 统计联合遮蔽时长
        detonate_times = [p[2] + p[3] for p in params]
        smoke_start = min(detonate_times)
        smoke_end = max([dt + smoke_valid_time for dt in detonate_times])

        total_duration = 0.0
        prev_effective = False
        t = smoke_start

        while t <= smoke_end + 1e-6:
            current_effective = False
            for i in range(NUM_SMOKE):
                if is_effective_occlusion(t, *params[i]):
                    current_effective = True
                    break

            if current_effective and not prev_effective:
                start_t = t
            elif not current_effective and prev_effective:
                total_duration += t - start_t

            prev_effective = current_effective
            t += self.time_step

        if prev_effective:
            total_duration += smoke_end - start_t

        return round(total_duration, 3)

    def pso_update(self, i):
        """
        PSO更新操作
        """
        # PSO速度更新
        r1, r2 = np.random.random(2)
        cognitive = self.c1 * r1 * (self.pbest[i] - self.population[i])
        social = self.c2 * r2 * (self.gbest - self.population[i])
        self.velocity[i] = self.w * self.velocity[i] + cognitive + social

        # 位置更新
        new_position = self.population[i] + self.velocity[i]

        # 边界处理和约束修复
        new_position = self.repair_solution(new_position)

        return new_position

    def de_operation(self, i):
        """
        DE操作：变异和交叉
        """
        # 随机选择三个不同的个体
        indices = [j for j in range(self.pop_size) if j != i]
        a, b, c = self.pbest[np.random.choice(indices, 3, replace=False)]

        # 差分变异
        mutant = a + self.F * (b - c)

        # 二项交叉
        trial = np.copy(self.population[i])
        cross_points = np.random.random(self.num_dimensions) < self.CR
        trial[cross_points] = mutant[cross_points]

        # 边界处理和约束修复
        trial = self.repair_solution(trial)

        return trial

    def optimize(self, max_generations, hybrid_ratio=0.3):
        """
        执行DE+PSO混合优化
        """
        print("开始DE+PSO混合优化...")
        start_time = time.time()

        # 初始评估
        for i in range(self.pop_size):
            fitness = self.fitness_function(self.population[i])
            self.pbest_fitness[i] = fitness

            if fitness > self.gbest_fitness:
                self.gbest = self.population[i].copy()
                self.gbest_fitness = fitness

        self.gbest_history.append(self.gbest_fitness)

        # 主优化循环
        for gen in range(max_generations):
            for i in range(self.pop_size):
                # DE+PSO混合策略：以hybrid_ratio概率使用DE，否则使用PSO
                if np.random.random() < hybrid_ratio:
                    # DE操作
                    trial = self.de_operation(i)
                    trial_fitness = self.fitness_function(trial)

                    # 贪婪选择
                    if trial_fitness > self.pbest_fitness[i]:
                        self.population[i] = trial
                        self.pbest[i] = trial
                        self.pbest_fitness[i] = trial_fitness

                        if trial_fitness > self.gbest_fitness:
                            self.gbest = trial.copy()
                            self.gbest_fitness = trial_fitness
                else:
                    # PSO操作
                    new_position = self.pso_update(i)
                    new_fitness = self.fitness_function(new_position)

                    if new_fitness > self.pbest_fitness[i]:
                        self.population[i] = new_position
                        self.pbest[i] = new_position
                        self.pbest_fitness[i] = new_fitness

                        if new_fitness > self.gbest_fitness:
                            self.gbest = new_position.copy()
                            self.gbest_fitness = new_fitness

            # 自适应调整参数
            self.w = 0.9 - (0.5 * gen / max_generations)  # 线性递减惯性权重

            self.gbest_history.append(self.gbest_fitness)

            if gen % 10 == 0:
                print(f"Generation {gen}: Best Fitness = {self.gbest_fitness:.4f}s")

        end_time = time.time()
        print(f"优化完成！总耗时: {end_time - start_time:.2f}秒")
        print(f"最优联合遮蔽时长: {self.gbest_fitness:.4f}s")

        return self.gbest, self.gbest_fitness

    def save_results(self, filename="result1.xlsx"):
        """
        保存结果到Excel文件
        """
        if self.gbest is None:
            print("请先运行优化！")
            return

        # 解析最优解
        results = []
        for i in range(NUM_SMOKE):
            base = i * 4
            drone_speed = self.gbest[base]
            theta = self.gbest[base + 1]
            t_drop = self.gbest[base + 2]
            t_detonate = self.gbest[base + 3]

            # 计算投放点和起爆点
            drop_x = self.drone_init_pos[0] + drone_speed * math.cos(theta) * t_drop
            drop_y = self.drone_init_pos[1] + drone_speed * math.sin(theta) * t_drop
            drop_z = self.drone_init_pos[2]

            explosion_point = get_smoke_explosion_center(
                self.drone_init_pos, drone_speed, theta, t_drop, t_detonate
            )

            results.append({
                '烟幕弹编号': i + 1,
                '无人机速度(m/s)': round(drone_speed, 2),
                '飞行方向角(°)': round(theta * 180 / math.pi, 2),
                '投放延迟(s)': round(t_drop, 2),
                '起爆延迟(s)': round(t_detonate, 2),
                '投放点X(m)': round(drop_x, 2),
                '投放点Y(m)': round(drop_y, 2),
                '投放点Z(m)': round(drop_z, 2),
                '起爆点X(m)': round(explosion_point[0], 2),
                '起爆点Y(m)': round(explosion_point[1], 2),
                '起爆点Z(m)': round(explosion_point[2], 2),
                '有效遮蔽时长(s)': round(self.single_durations[i], 2)
            })

        # 创建DataFrame并保存
        df = pd.DataFrame(results)
        df.to_excel(filename, index=False)
        print(f"结果已保存到 {filename}")

        return df


# -------------------------- 3. 主程序 --------------------------
if __name__ == "__main__":
    # 定义优化问题参数
    pop_size = 50
    num_dimensions = 4 * NUM_SMOKE  # 12个变量

    # 定义变量边界
    bounds = []
    for i in range(NUM_SMOKE):
        bounds.extend([
            (70, 140),  # 无人机速度
            (0, 2 * math.pi),  # 方向角
            (0, 30),  # 投放延迟
            (0, 10)  # 起爆延迟
        ])

    # 创建DE+PSO混合优化器
    optimizer = DE_PSO_Hybrid(
        pop_size=pop_size,
        num_dimensions=num_dimensions,
        bounds=bounds,
        drone_init_pos=drone_FY1
    )

    # 执行优化
    best_solution, best_fitness = optimizer.optimize(
        max_generations=100,
        hybrid_ratio=0.3  # 30%概率使用DE，70%概率使用PSO
    )

    # 保存结果
    result_df = optimizer.save_results("result1.xlsx")

    # 输出详细结果
    print("\n最优投放策略:")
    for i in range(NUM_SMOKE):
        base = i * 4
        drone_speed = best_solution[base]
        theta = best_solution[base + 1]
        t_drop = best_solution[base + 2]
        t_detonate = best_solution[base + 3]
        theta_deg = (theta * 180 / math.pi) % 360

        print(f"烟幕弹{i + 1}:")
        print(f"  速度={drone_speed:.2f}m/s, 方向角={theta_deg:.2f}°")
        print(f"  投放延迟={t_drop:.2f}s, 起爆延迟={t_detonate:.2f}s")
        print(f"  有效遮蔽时长={optimizer.single_durations[i]:.2f}s")

    print(f"联合遮蔽总时长: {best_fitness:.2f}s")